
#include <AEEStdErr.h>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <math.h>
#include <rpcmem.h>
#include <semaphore.h>
#include <assert.h>
#include <unistd.h>
#include <time.h>
#include "dsp_capabilities_utils.h"     // available under <HEXAGON_SDK_ROOT>/utils/examples
#include "os_defines.h"

/* Default configuration */

#include "tracker_kcf.h"

#include "tracker_kcf_dsp_adap.h"

#if defined( __ANDROID__ )
#include <android/log.h>
#define LOG_TAG    "dsp_blob"
#define LOGD(...)  __android_log_print(ANDROID_LOG_DEBUG,LOG_TAG,__VA_ARGS__)
#define LOGI(...)  __android_log_print(ANDROID_LOG_INFO,LOG_TAG,__VA_ARGS__)
#define LOGE(...)  __android_log_print(ANDROID_LOG_ERROR,LOG_TAG,__VA_ARGS__)
#else
#define LOGD(...)  printf(__VA_ARGS__)
#define LOGI(...)  printf(__VA_ARGS__)
#define LOGE(...)  printf(__VA_ARGS__)
#endif


static void error(char *msg) {
    LOGE("ERROR: %s\n", msg);
    exit(EXIT_FAILURE);
}

static void error_code_fail(const char *file, int line, const char *expr, int code) {
    LOGE("%s(%d): %s failed with 0x%x\n", file, line, expr, code);
    exit(EXIT_FAILURE);
}

static void check_fail(const char *file, int line, const char *expr) {
    LOGE("%s(%d): %s failed\n", file, line, expr);
    exit(EXIT_FAILURE);
}

#define TEST_ERROR_CODE(x) { int e; if ( (e = x) != 0 ) { error_code_fail(__FILE__, __LINE__, #x, e); } }
#define CHECK(x) { if ( !(x) ) { check_fail(__FILE__, __LINE__, #x); } }

const char * nctid_tracker_kcf_get_version()
{
    static char version[] = {"v0.0.2@" __DATE__ "-" __TIME__};
    return version;
}

typedef struct {
    void *addr;
    uint32_t offset;
    uint32_t len;
    int fd;
} mmap_buffer_t;

typedef struct {
    remote_handle64 handle;
    mmap_buffer_t buffer;
} kcf_handle_t;

void * nctid_tracker_kcf_open_session(int unsignedpd_flag)
{
    int retVal = 0;
    remote_handle64 handle = -1;
    int tracker_kcf_URI_domain_len = strlen(tracker_kcf_URI) + MAX_DOMAIN_URI_SIZE;
    char *tracker_kcf_URI_domain = NULL;
    domain *my_domain = NULL;
    uint64_t t;
    kcf_handle_t *kcf_handle = NULL;

    // int domain_id = -1;
    int domain_id = 3; // -1;
    // int unsignedpd_flag = 0;
    bool is_unsignedpd_enabled = false;
    if ( domain_id == -1 ) {
        LOGI("\nDSP domain is not provided. Retrieving DSP information using Remote APIs.\n");
        retVal = get_dsp_support(&domain_id);
        if (retVal != AEE_SUCCESS) {
            LOGI("ERROR in get_dsp_support: 0x%x, defaulting to CDSP domain\n", retVal);
        }
    }

    if (!is_valid_domain_id(domain_id, 1)) {
        retVal = AEE_EBADPARM;
        LOGI("\nInvalid domain %d\n", domain_id);
        goto bail;
    }

    LOGI("usingedpd_flag = %d", unsignedpd_flag);
    if (unsignedpd_flag < 0 || unsignedpd_flag > 1) {
        retVal = AEE_EBADPARM;
        LOGI("\nInvalid unsigned PD flag %d\n", unsignedpd_flag);
        goto bail;
    }
    if(unsignedpd_flag == 1) {
        is_unsignedpd_enabled = is_unsignedpd_supported(domain_id);
    }
    if(is_unsignedpd_enabled) {
        if(remote_session_control) {
            struct remote_rpc_control_unsigned_module data;
            data.domain = domain_id;
            data.enable = 1;
            LOGI("remote_session_control set DSPRPC_CONTROL_UNSIGNED_MODULE\n");
            if (AEE_SUCCESS != (retVal = remote_session_control(DSPRPC_CONTROL_UNSIGNED_MODULE, (void*)&data, sizeof(data)))) {
                LOGI("ERROR 0x%x: remote_session_control failed\n", retVal);
                goto bail;
            }
        }
        else {
            retVal = AEE_EUNSUPPORTED;
            LOGI("ERROR 0x%x: remote_session_control interface is not supported on this device\n", retVal);
            goto bail;
        }
    }

    my_domain = get_domain(domain_id);
    if (my_domain == NULL) {
        retVal = AEE_EBADPARM;
        LOGI("\nERROR 0x%x: unable to get domain struct %d\n", retVal, domain_id);
        goto bail;
    }

    if ((tracker_kcf_URI_domain = (char *)malloc(tracker_kcf_URI_domain_len)) == NULL) {
        retVal = AEE_ENOMEMORY;
        LOGI("unable to allocated memory for tracker_kcf_URI_domain of size: %d", tracker_kcf_URI_domain_len);
        goto bail;
    }

    retVal = snprintf(tracker_kcf_URI_domain, tracker_kcf_URI_domain_len, "%s%s", tracker_kcf_URI, my_domain->uri);
    if (retVal < 0) {
        LOGI("ERROR 0x%x returned from snprintf\n", retVal);
        retVal = AEE_EFAILED;
        goto bail;
    }
    LOGI("URI_domain: %s\n", tracker_kcf_URI_domain);
    /*Open a handle on the CDSP*/
    retVal = tracker_kcf_open(tracker_kcf_URI_domain, &handle);
    if (retVal) {
        LOGI("ERROR 0x%x: unable to create fastrpc session on CDSP\n", retVal);
        goto bail;
    }

    /* fcvqueue expects the client to initialize rpcmem */
    rpcmem_init();

    kcf_handle = malloc(sizeof(kcf_handle_t));
    if (kcf_handle == NULL) {
        goto bail;
    }
    kcf_handle->handle = handle;
    return kcf_handle;
bail:
    if (tracker_kcf_URI_domain) {
        free(tracker_kcf_URI_domain);
    }
    if(handle != -1)
        tracker_kcf_close(handle);

    /* Uninitialize rpcmem */
    rpcmem_deinit();

    return NULL;

}

void nctid_tracker_kcf_close_session(void *handle)
{
    if (handle == NULL) {
        return;
    }
    kcf_handle_t *kcf_handle = (kcf_handle_t*)handle;
    tracker_kcf_close(kcf_handle->handle);

    if (kcf_handle->buffer.addr != NULL) {
        rpcmem_free(kcf_handle->buffer.addr);
    }
    /* Uninitialize rpcmem */
    rpcmem_deinit();
}

int nctid_tracker_kcf_init(void *handle, int img_width, int img_height, int patch_size, bool multiscale)
{
    kcf_handle_t *kcf_handle = (kcf_handle_t*)handle;
    int len = img_height*img_width;
    void *ion_buffer = rpcmem_alloc(RPCMEM_HEAP_ID_SYSTEM, RPCMEM_DEFAULT_FLAGS, len);
    CHECK(ion_buffer != NULL);
    int fd = rpcmem_to_fd(ion_buffer);
    if (fd == -1) {
        rpcmem_free(ion_buffer);
        CHECK(fd == -1);
    }
    kcf_handle->buffer.addr = ion_buffer;
    kcf_handle->buffer.fd = fd;
    kcf_handle->buffer.len = len;
    kcf_handle->buffer.offset = 0;
    TEST_ERROR_CODE(tracker_kcf_set_clocks(kcf_handle->handle));
    TEST_ERROR_CODE(tracker_kcf_init(kcf_handle->handle, img_width, img_height, patch_size, multiscale));
    TEST_ERROR_CODE(tracker_kcf_mem_dmahandle(kcf_handle->handle, fd, 0, len));
    return 0;
}

int nctid_tracker_kcf_update(void *handle, struct nctid_roi_t *roi, uint8_t *image, size_t size, float train_interp_factor, int32_t *dsp_usec)
{
    int32_t usec;
    roi_t roi_;
    kcf_handle_t *kcf_handle = (kcf_handle_t*)handle;
    roi_.x = roi->x;
    roi_.y = roi->y;
    roi_.width = roi->width;
    roi_.height = roi->height;
    memcpy(kcf_handle->buffer.addr, image, size);
    TEST_ERROR_CODE(tracker_kcf_update(kcf_handle->handle, &roi_, kcf_handle->buffer.fd, train_interp_factor, &usec));
    if (dsp_usec) {
        *dsp_usec = usec;
    }
    return 0;
} 

int nctid_tracker_kcf_update2(void *handle, struct nctid_roi_t *roi, float train_interp_factor, int32_t *dsp_usec)
{
    int32_t usec;
    roi_t roi_;
    kcf_handle_t *kcf_handle = (kcf_handle_t*)handle;
    roi_.x = roi->x;
    roi_.y = roi->y;
    roi_.width = roi->width;
    roi_.height = roi->height;
    TEST_ERROR_CODE(tracker_kcf_update2(kcf_handle->handle, &roi_, kcf_handle->buffer.fd, train_interp_factor, &usec));
    if (dsp_usec) {
        *dsp_usec = usec;
    }
    return 0;
}

int nctid_tracker_kcf_detect(void *handle, struct nctid_roi_t *in_roi, struct nctid_roi_t *out_roi, uint8_t *in_image, size_t size, float *peak_value, int32_t *dsp_usec)
{
    int32_t usec;
    roi_t in_roi_, out_roi_;
    kcf_handle_t *kcf_handle = (kcf_handle_t*)handle;
    in_roi_.x = in_roi->x;
    in_roi_.y = in_roi->y;
    in_roi_.width = in_roi->width;
    in_roi_.height = in_roi->height;
    memcpy(kcf_handle->buffer.addr, in_image, size);
    TEST_ERROR_CODE(tracker_kcf_detect(kcf_handle->handle, &in_roi_, kcf_handle->buffer.fd, &out_roi_, peak_value, &usec));  
    out_roi->x = out_roi_.x;
    out_roi->y = out_roi_.y;
    out_roi->width = out_roi_.width;
    out_roi->height = out_roi_.height;
    if (dsp_usec) {
        *dsp_usec = usec;
    }
    return 0;
} 


void nctid_tracker_kcf_get_debug(void *handle, uint8_t *data, size_t size)
{
    kcf_handle_t *kcf_handle = (kcf_handle_t*)handle;
    TEST_ERROR_CODE(tracker_kcf_get_debug(kcf_handle->handle, data, size));
}